from typing import Any, Dict, List, Optional
from fastapi import APIRouter, Depends, HTTPException, Query, Path, Body, Request
from sqlalchemy.orm import Session
import json
import requests
from datetime import datetime
import base64
import asyncio
import time
from typing import Optional, List

from database import get_db
from models import Drone, User
from schemas import ResponseModel
from api.deps import get_current_active_user, get_client_ip
from utils import log_operation
from config import ZENOH_BASE_URL

router = APIRouter()

# 存储无人机任务监控状态
monitored_drones = {}

# 定时监控任务状态和位置的协程
async def monitor_drone_status(drone_id: str, mission_id: int, monitoring_interval: float = 2.0, max_duration: float = 3600.0):
    """
    定时监控无人机状态和位置信息
    
    Args:
        drone_id: 无人机ID
        mission_id: 任务ID
        monitoring_interval: 监控间隔（秒）
        max_duration: 最大监控时长（秒），默认1小时
    """
    print(f"开始监控无人机 {drone_id} 的任务 {mission_id}")
    start_time = time.time()
    zenoh_client = ZenohClient(ZENOH_BASE_URL)
    
    # 初始化监控状态
    monitored_drones[drone_id] = {
        'mission_id': mission_id,
        'is_monitoring': True,
        'positions': [],
        'statuses': [],
        'start_time': start_time
    }
    
    try:
        while time.time() - start_time < max_duration:
            # 获取位置信息
            pose_key = f"web/{drone_id}/telemetry/pose"
            pose_response = zenoh_client.get(pose_key)
            if pose_response["success"]:
                try:
                    pose_data = json.loads(pose_response.get("payload", "{}") or "{}")
                    # 保存位置数据
                    monitored_drones[drone_id]['positions'].append({
                        'timestamp': time.time(),
                        'position': pose_data.get('position', {})
                    })
                    print(f"[DEBUG] 获取到无人机 {drone_id} 位置: {pose_data.get('position', {})}")
                except Exception as e:
                    print(f"[DEBUG] 解析位置数据失败: {str(e)}")
            
            # 获取状态信息
            status_key = f"web/{drone_id}/mission_status"
            status_response = zenoh_client.get(status_key)
            if status_response["success"]:
                try:
                    status_data = json.loads(status_response.get("payload", "{}") or "{}")
                    # 保存状态数据
                    monitored_drones[drone_id]['statuses'].append({
                        'timestamp': time.time(),
                        'status': status_data
                    })
                    print(f"[DEBUG] 获取到无人机 {drone_id} 任务状态: {status_data.get('state', 'unknown')}")
                    
                    # 检查任务是否完成
                    if status_data.get('state') == 'completed':
                        print(f"[DEBUG] 无人机 {drone_id} 任务 {mission_id} 已完成，停止监控")
                        break
                except Exception as e:
                    print(f"[DEBUG] 解析状态数据失败: {str(e)}")
            
            # 等待下一次监控
            await asyncio.sleep(monitoring_interval)
    except asyncio.CancelledError:
        print(f"无人机 {drone_id} 的监控任务被取消")
    except Exception as e:
        print(f"监控无人机 {drone_id} 时发生错误: {str(e)}")
    finally:
        # 更新监控状态
        if drone_id in monitored_drones:
            monitored_drones[drone_id]['is_monitoring'] = False
            monitored_drones[drone_id]['end_time'] = time.time()
        print(f"停止监控无人机 {drone_id} 的任务 {mission_id}")

class ZenohClient:
    """
    Zenoh REST客户端，用于与Zenoh服务进行通信
    """
    
    def __init__(self, base_url: str):
        """
        初始化Zenoh客户端
        
        Args:
            base_url: Zenoh REST服务的基础URL
        """
        self.base_url = base_url.rstrip('/')
    
    def build_url(self, key: str) -> str:
        """
        构建完整的Zenoh REST URL
        
        Args:
            key: Zenoh键
            
        Returns:
            完整的URL字符串
        """
        clean_key = key.lstrip('/')
        return f"{self.base_url}/{clean_key}"
    
    def put(self, key: str, payload: Dict[str, Any]) -> Dict[str, Any]:
        """
        向Zenoh发布数据
        
        Args:
            key: Zenoh键
            payload: 要发布的数据
            
        Returns:
            响应结果
        """
        url = self.build_url(key)
        headers = {"Content-Type": "application/json; charset=utf-8"}
        
        print(f"[DEBUG] 发送PUT请求到 {url}")
        print(f"[DEBUG] 请求数据: {json.dumps(payload, ensure_ascii=False, indent=2)}")
        
        try:
            response = requests.put(url, json=payload, headers=headers)
            response.raise_for_status()
            print(f"[DEBUG] PUT响应状态码: {response.status_code}")
            print(f"[DEBUG] PUT响应内容: {response.text}")
            return {"success": True, "status_code": response.status_code, "text": response.text}
        except requests.RequestException as e:
            error_message = str(e)
            print(f"[DEBUG] PUT请求失败: {error_message}")
            return {"success": False, "error": error_message}
    
    def get(self, key: str) -> Dict[str, Any]:
        """
        从Zenoh获取数据
        
        Args:
            key: Zenoh键
            
        Returns:
            响应结果
        """
        url = self.build_url(key)
        
        print(f"[DEBUG] 发送GET请求到 {url}")
        
        try:
            response = requests.get(url, headers={"Cache-Control": "no-store"})
            response.raise_for_status()
            print(f"[DEBUG] GET响应状态码: {response.status_code}")
            print(f"[DEBUG] GET响应内容: {response.text}")
            return {"success": True, "status_code": response.status_code, "text": response.text}
        except requests.RequestException as e:
            error_message = str(e)
            print(f"[DEBUG] GET请求失败: {error_message}")
            return {"success": False, "error": error_message}

    @staticmethod
    def extract_payload(response_text: str) -> str:
        """
        从Zenoh REST响应中提取有效载荷
        
        Args:
            response_text: Zenoh REST响应文本
            
        Returns:
            提取的有效载荷文本
        """
        try:
            # 尝试解析为JSON
            data = json.loads(response_text)
            
            # Zenoh REST常见格式：[{ key, value, encoding, timestamp }]
            if isinstance(data, list):
                if len(data) == 0:
                    return ""
                
                entry = data[0]
                enc = entry.get("encoding", "")
                val = entry.get("value", "")
                
                if enc.startswith("zenoh/bytes"):
                    # 解码base64编码的bytes
                    try:
                        decoded_bytes = base64.b64decode(val)
                        return decoded_bytes.decode('utf-8')
                    except Exception:
                        return str(val)
                elif not isinstance(val, str):
                    # 如果值不是字符串，转换为JSON字符串
                    return json.dumps(val)
                
                return str(val)
            
            # 直接是对象，返回原始文本
            return response_text
        except json.JSONDecodeError:
            # 不是JSON，返回原始文本
            return response_text

def get_default_telemetry_data(telemetry_type: str) -> Dict[str, Any]:
    """
    返回默认的遥测数据（简化版，去除header）

    Args:
        telemetry_type: 遥测数据类型

    Returns:
        默认的遥测数据
    """
    defaults = {
        "pose": {
            "position": {"x": 0.0, "y": 0.0, "z": 0.0},
            "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
        },
        "twist": {
            "linear": {"x": 0.0, "y": 0.0, "z": 0.0},
            "angular": {"x": 0.0, "y": 0.0, "z": 0.0}
        },
        "battery": {
            "voltage": 16.8,
            "percentage": 1.0,
            "status": 2,  # DISCHARGING
            "health": 1,  # GOOD
            "present": True
        },
        "gps": {
            "latitude": 0.0,
            "longitude": 0.0,
            "altitude": 0.0,
            "status": -1
        },
        "imu": {
            "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0},
            "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
            "linear_acceleration": {"x": 0.0, "y": 0.0, "z": 9.8}
        },
        "alert": {
            "alert": 0,
            "description": ""
        },
        "platform_info": {
            "status": 0,
            "connected": False,
            "armed": False
        }
    }

    return defaults.get(telemetry_type, {})

def extract_telemetry_data(raw_data: Dict[str, Any], telemetry_type: str) -> Dict[str, Any]:
    """
    从原始遥测数据中提取简化的数据（去除header和冗余字段）

    Args:
        raw_data: 原始遥测数据
        telemetry_type: 遥测数据类型

    Returns:
        简化后的遥测数据
    """
    if not raw_data:
        return get_default_telemetry_data(telemetry_type)

    try:
        if telemetry_type == "pose":
            pose_data = raw_data.get("pose", raw_data)
            return {
                "position": pose_data.get("position", {"x": 0.0, "y": 0.0, "z": 0.0}),
                "orientation": pose_data.get("orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0})
            }

        elif telemetry_type == "twist":
            twist_data = raw_data.get("twist", raw_data)
            return {
                "linear": twist_data.get("linear", {"x": 0.0, "y": 0.0, "z": 0.0}),
                "angular": twist_data.get("angular", {"x": 0.0, "y": 0.0, "z": 0.0})
            }

        elif telemetry_type == "battery":
            return {
                "voltage": raw_data.get("voltage", 16.8),
                "percentage": raw_data.get("percentage", 1.0),
                "status": raw_data.get("power_supply_status", 2),
                "health": raw_data.get("power_supply_health", 1),
                "present": raw_data.get("present", True)
            }

        elif telemetry_type == "gps":
            status_data = raw_data.get("status", {})
            return {
                "latitude": raw_data.get("latitude", 0.0),
                "longitude": raw_data.get("longitude", 0.0),
                "altitude": raw_data.get("altitude", 0.0),
                "status": status_data.get("status", -1) if isinstance(status_data, dict) else -1
            }

        elif telemetry_type == "imu":
            return {
                "orientation": raw_data.get("orientation", {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}),
                "angular_velocity": raw_data.get("angular_velocity", {"x": 0.0, "y": 0.0, "z": 0.0}),
                "linear_acceleration": raw_data.get("linear_acceleration", {"x": 0.0, "y": 0.0, "z": 9.8})
            }

        elif telemetry_type == "alert":
            return {
                "alert": raw_data.get("alert", 0),
                "description": raw_data.get("description", "")
            }

        elif telemetry_type == "platform_info":
            return {
                "status": raw_data.get("status", 0),
                "connected": raw_data.get("connected", False),
                "armed": raw_data.get("armed", False)
            }

        else:
            return raw_data

    except Exception:
        return get_default_telemetry_data(telemetry_type)

def build_mission_object(ns: str, speed: float, takeoff_height: float,
                       yaw_mode: int, yaw_angle: float, path: List[List[float]],
                       is_gps: bool, end_z: float = 0.8) -> Dict[str, Any]:
    """
    构建任务对象
    
    Args:
        ns: 目标命名空间
        speed: 飞行速度
        takeoff_height: 起飞高度
        yaw_mode: 偏航模式
        yaw_angle: 偏航角度
        path: 路径点列表
        is_gps: 是否使用GPS
        end_z: 结束高度
        
    Returns:
        任务对象字典
    """
    plan = []
    
    # 起飞动作
    plan.append({
        "behavior": "takeoff",
        "method": "__call__",
        "args": {
            "height": takeoff_height,
            "speed": speed,
            "wait": True
        }
    })
    
    # 路径跟踪动作
    if is_gps:
        plan.append({
            "behavior": "follow_path_gps",
            "method": "__call__",
            "args": {
                "geopath": path,
                "speed": speed,
                "yaw_mode": yaw_mode,
                "yaw_angle": yaw_angle,
                "wait": True
            }
        })
    else:
        plan.append({
            "behavior": "follow_path",
            "method": "__call__",
            "args": {
                "path": path,
                "speed": speed,
                "yaw_mode": yaw_mode,
                "yaw_angle": yaw_angle,
                "wait": True
            }
        })
    
    # 降落动作
    plan.append({
        "behavior": "go_to",
        "method": "__call__",
        "args": {
            "x": 0.0,
            "y": 0.0,
            "z": end_z,
            "speed": 1.0,
            "yaw_mode": 0,
            "yaw_angle": 0.0,
            "wait": True
        }
    })
    
    return {"target": ns, "plan": plan}

@router.put("/mission_update", response_model=ResponseModel[Dict[str, Any]])
def px4_mission_update(
    request: Request,
    payload: Dict[str, Any] = Body(..., description="任务更新载荷"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    print(f"[DEBUG] 接收到任务更新请求")
    print(f"[DEBUG] 任务更新数据: {json.dumps(payload, ensure_ascii=False, indent=2)}")
    """
    PX4 任务更新接口
    
    处理来自 Zenoh REST 的任务更新请求，支持 LOAD（加载任务）和 START（启动任务）操作
    
    Args:
        request: HTTP请求对象
        payload: 任务更新载荷，包含drone_id, mission_id, action等字段
        current_user: 当前用户
        db: 数据库会话
        
    Returns:
        操作结果
    """
    try:
        # 解析载荷数据
        drone_id = payload.get("drone_id")
        mission_id = payload.get("mission_id")
        action = payload.get("action")
        mission_data = payload.get("mission")
        
        # 如果没有mission_data，尝试从path字段获取（向后兼容）
        if not mission_data:
            mission_data = payload.get("path")
        
        # 验证必要参数
        if not drone_id or not mission_id or not action:
            raise HTTPException(
                status_code=400,
                detail="缺少必要参数: drone_id, mission_id, action"
            )
        
        # 处理字符串形式的action参数
        if isinstance(action, str):
            action = action.upper()
            if action == "LOAD":
                action = 1
            elif action == "START":
                action = 2
            else:
                raise HTTPException(
                    status_code=400,
                    detail=f"无效的action值: {action}，应为LOAD或START"
                )
        
        # 验证无人机是否存在
        drone = db.query(Drone).filter(Drone.id == drone_id).first()
        if not drone:
            # 如果是字符串形式的drone_id，尝试查找名称匹配的无人机
            if isinstance(drone_id, str):
                drone = db.query(Drone).filter(Drone.name == drone_id).first()
            
            # 对于测试环境，可以考虑跳过无人机存在验证
            # 在实际生产环境中应该严格验证
            if not drone:
                print(f"[DEBUG] 警告: 无人机ID {drone_id} 不存在于数据库中，使用模拟无人机")
                # 不抛出404异常，而是创建一个模拟的无人机对象
                drone = Drone()
                drone.id = drone_id
                drone.name = drone_id
                drone.status = "available"
                drone.battery_level = 100
        
        action_text = "LOAD" if action == 1 else "START" if action == 2 else f"未知操作({action})"
        result = {"success": True, "message": f"{action_text} 操作成功"}
        
        # 处理不同的操作类型
        if action == 1:  # LOAD 操作
            if not mission_data:
                raise HTTPException(
                    status_code=400,
                    detail="LOAD 操作需要 mission 数据"
                )
            
            # 记录 LOAD 操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="px4_mission_load",
                operation_content=f"用户 {current_user.username} 加载任务 {mission_id} 到无人机 {drone.name}",
                success=1
            )
            
        elif action == 2:  # START 操作
            # 检查无人机状态
            if drone.status == "maintenance":
                raise HTTPException(
                    status_code=400,
                    detail="无人机正在维护中，无法执行任务"
                )
            
            # 检查电池电量
            if drone.battery_level is not None and drone.battery_level < 20:
                raise HTTPException(
                    status_code=400,
                    detail=f"无人机电池电量过低({drone.battery_level}%)，无法执行任务"
                )
            
            # 更新无人机状态
            drone.status = "busy"
            drone.last_flight_time = datetime.now()
            db.commit()
            
            # 记录 START 操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="px4_mission_start",
                operation_content=f"用户 {current_user.username} 启动无人机 {drone.name} 的任务 {mission_id}",
                success=1
            )
        
        return ResponseModel(data=result)
        
    except HTTPException:
        raise
    except Exception as e:
        print(f"PX4 任务更新失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="任务更新时发生错误"
        )

@router.get("/telemetry/{drone_id}/pose", response_model=ResponseModel[Dict[str, Any]])
def get_px4_telemetry_pose(
    drone_id: str = Path(..., description="无人机ID，通常为uav_1等字符串格式"),
    zenoh_base_url: str = Query(ZENOH_BASE_URL, description="Zenoh REST服务基础URL"),
    current_user: User = Depends(get_current_active_user)
) -> Any:
    print(f"[DEBUG] 接收到获取姿态数据请求，无人机ID: {drone_id}")
    """
    获取PX4无人机的姿态遥测数据
    
    通过Zenoh REST接口获取无人机的位置和姿态数据
    
    Args:
        drone_id: 无人机ID，通常为uav_1等字符串格式
        zenoh_base_url: Zenoh REST服务基础URL
        current_user: 当前用户
        
    Returns:
        无人机姿态遥测数据，包含位置和姿态信息
    """
    try:
        # 初始化Zenoh客户端
        zenoh_client = ZenohClient(zenoh_base_url)
        
        # 构建遥测数据key
        telemetry_key = f"web/{drone_id}/telemetry/pose"
        
        # 从Zenoh获取遥测数据
        response = zenoh_client.get(telemetry_key)
        
        if not response["success"]:
            # 如果获取失败，返回模拟数据
            return ResponseModel(
                data={
                    "frame_id": "map",
                    "stamp": {
                        "sec": int(datetime.now().timestamp()),
                        "nanosec": 0
                    },
                    "position": {
                        "x": 0.0,
                        "y": 0.0,
                        "z": 10.0
                    },
                    "orientation": {
                        "x": 0.0,
                        "y": 0.0,
                        "z": 0.0,
                        "w": 1.0
                    },
                    "velocity": {
                        "x": 0.0,
                        "y": 0.0,
                        "z": 0.0
                    }
                }
            )
        
        # 提取有效载荷
        payload_text = zenoh_client.extract_payload(response["text"])
        
        # 解析为JSON
        payload = json.loads(payload_text)
        
        return ResponseModel(data=payload)
        
    except Exception as e:
        print(f"获取PX4遥测数据失败: {str(e)}")
        # 返回模拟数据作为备用
        return ResponseModel(
            data={
                "frame_id": "map",
                "stamp": {
                    "sec": int(datetime.now().timestamp()),
                    "nanosec": 0
                },
                "position": {
                    "x": 0.0,
                    "y": 0.0,
                    "z": 10.0
                },
                "orientation": {
                    "x": 0.0,
                    "y": 0.0,
                    "z": 0.0,
                    "w": 1.0
                },
                "velocity": {
                    "x": 0.0,
                    "y": 0.0,
                    "z": 0.0
                }
            }
        )

@router.get("/mission_status/{drone_id}", response_model=ResponseModel[Dict[str, Any]])
def get_px4_mission_status(
    drone_id: str = Path(..., description="无人机ID，通常为uav_1等字符串格式"),
    zenoh_base_url: str = Query(ZENOH_BASE_URL, description="Zenoh REST服务基础URL"),
    current_user: User = Depends(get_current_active_user)
) -> Any:
    print(f"[DEBUG] 接收到获取任务状态请求，无人机ID: {drone_id}")
    """
    获取PX4无人机的任务状态
    
    通过Zenoh REST接口获取无人机当前的任务执行状态
    
    Args:
        drone_id: 无人机ID，通常为uav_1等字符串格式
        zenoh_base_url: Zenoh REST服务基础URL
        current_user: 当前用户
        
    Returns:
        无人机任务状态数据，包含任务ID、状态、进度等信息
    """
    try:
        # 初始化Zenoh客户端
        zenoh_client = ZenohClient(zenoh_base_url)
        
        # 构建任务状态key
        status_key = f"web/{drone_id}/mission_status"
        
        # 从Zenoh获取任务状态数据
        response = zenoh_client.get(status_key)
        
        if not response["success"]:
            # 如果获取失败，返回默认状态
            return ResponseModel(
                data={
                    "mission_id": 5001,
                    "status": "active",
                    "progress": 0,
                    "current_waypoint": 0,
                    "total_waypoints": 5,
                    "start_time": datetime.now().isoformat(),
                    "estimated_end_time": None
                }
            )
        
        # 提取有效载荷
        payload_text = zenoh_client.extract_payload(response["text"])
        
        # 解析为JSON
        payload = json.loads(payload_text)
        
        return ResponseModel(data=payload)
        
    except Exception as e:
        print(f"获取PX4任务状态失败: {str(e)}")
        # 返回默认状态作为备用
        return ResponseModel(
            data={
                "mission_id": 5001,
                "status": "active",
                "progress": 0,
                "current_waypoint": 0,
                "total_waypoints": 5,
                "start_time": datetime.now().isoformat(),
                "estimated_end_time": None
            }
        )

@router.post("/send_mission", response_model=ResponseModel[Dict[str, Any]])
async def send_px4_mission(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    mission_id: int = Body(..., description="任务ID"),
    zenoh_base_url: str = Body(ZENOH_BASE_URL, description="Zenoh REST服务基础URL"),
    zenoh_key_prefix: str = Body("rt", description="Zenoh键前缀"),
    gps_points: Optional[List[List[float]]] = Body(None, description="GPS路径点列表"),
    map_points: Optional[List[List[float]]] = Body(None, description="地图路径点列表"),
    speed: float = Body(3.0, description="飞行速度"),
    takeoff_height: float = Body(10.0, description="起飞高度"),
    yaw_mode: int = Body(0, description="偏航模式"),
    yaw_angle: float = Body(0.0, description="偏航角度"),
    end_z: float = Body(0.8, description="结束高度"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    print(f"[DEBUG] 接收到发送任务请求，无人机ID: {drone_id}, 任务ID: {mission_id}")
    print(f"[DEBUG] 任务参数 - 速度: {speed}, 起飞高度: {takeoff_height}, 偏航模式: {yaw_mode}")
    print(f"[DEBUG] GPS路径点数量: {len(gps_points) if gps_points else 0}")
    print(f"[DEBUG] 地图路径点数量: {len(map_points) if map_points else 0}")
    """
    发送PX4任务到无人机
    
    构建任务对象并通过Zenoh发送到无人机
    
    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        mission_id: 任务ID
        zenoh_base_url: Zenoh REST服务基础URL
        zenoh_key_prefix: Zenoh键前缀
        gps_points: GPS路径点列表
        map_points: 地图路径点列表
        speed: 飞行速度
        takeoff_height: 起飞高度
        yaw_mode: 偏航模式
        yaw_angle: 偏航角度
        end_z: 结束高度
        current_user: 当前用户
        db: 数据库会话
        
    Returns:
        发送结果
    """
    try:
        # 初始化Zenoh客户端
        zenoh_client = ZenohClient(zenoh_base_url)
        
        # 构建任务对象
        if gps_points:
            print(f"[DEBUG] 开始构建GPS任务对象，ID: {mission_id}, 速度: {speed}")
            mission_obj = build_mission_object(
                ns=drone_id,
                speed=speed,
                takeoff_height=takeoff_height,
                yaw_mode=yaw_mode,
                yaw_angle=yaw_angle,
                path=gps_points,
                is_gps=True,
                end_z=end_z
            )
            print(f"[DEBUG] 构建的GPS任务对象: {json.dumps(mission_obj, ensure_ascii=False, indent=2)}")
        elif map_points:
            print(f"[DEBUG] 开始构建地图任务对象，ID: {mission_id}, 速度: {speed}")
            mission_obj = build_mission_object(
                ns=drone_id,
                speed=speed,
                takeoff_height=takeoff_height,
                yaw_mode=yaw_mode,
                yaw_angle=yaw_angle,
                path=map_points,
                is_gps=False,
                end_z=end_z
            )
            print(f"[DEBUG] 构建的地图任务对象: {json.dumps(mission_obj, ensure_ascii=False, indent=2)}")
        else:
            raise HTTPException(
                status_code=400,
                detail="必须提供GPS路径点或地图路径点"
            )
        
        # 构建LOAD载荷
        load_payload = {
            "drone_id": drone_id,
            "mission_id": mission_id,
            "action": 1,
            "mission": json.dumps(mission_obj)
        }
        
        # 构建START载荷
        start_payload = {
            "drone_id": drone_id,
            "mission_id": mission_id,
            "action": 2
        }
        
        # 构建Zenoh键
        zenoh_key = f"{zenoh_key_prefix}/{drone_id}/mission_update"
        
        # 发送LOAD操作
        print(f"[DEBUG] 准备发送LOAD操作到无人机 {drone_id}")
        load_response = zenoh_client.put(zenoh_key, load_payload)
        if not load_response["success"]:
            error_detail = f"LOAD操作失败: {load_response.get('error', '未知错误')}"
            print(f"[DEBUG] {error_detail}")
            raise HTTPException(
                status_code=500,
                detail=error_detail
            )
        print(f"[DEBUG] LOAD操作发送成功")
        
        # 等待短暂时间后发送START操作
        await asyncio.sleep(0.6)
        
        print(f"[DEBUG] 准备发送START操作到无人机 {drone_id}")
        start_response = zenoh_client.put(zenoh_key, start_payload)
        if not start_response["success"]:
            error_detail = f"START操作失败: {start_response.get('error', '未知错误')}"
            print(f"[DEBUG] {error_detail}")
            raise HTTPException(
                status_code=500,
                detail=error_detail
            )
        print(f"[DEBUG] START操作发送成功")
        
        # 启动定时监控任务，飞机起飞后开始定时获取位置和状态信息
        print(f"任务 {mission_id} 已开始，启动无人机 {drone_id} 的位置和状态监控")
        # 创建并运行监控协程（非阻塞）
        asyncio.create_task(monitor_drone_status(
            drone_id=drone_id,
            mission_id=mission_id,
            monitoring_interval=2.0,  # 每2秒获取一次数据
            max_duration=3600.0  # 最多监控1小时
        ))
        
        # 记录操作
        log_operation(
            db=db,
            user_id=current_user.id,
            username=current_user.username,
            ip_address=get_client_ip(request),
            operation_type="send_px4_mission",
            operation_content=f"用户 {current_user.username} 发送任务 {mission_id} 到无人机 {drone_id}",
            success=1
        )
        
        return ResponseModel(
            data={
                "success": True,
                "message": "任务发送成功",
                "drone_id": drone_id,
                "mission_id": mission_id
            }
        )
        
    except HTTPException:
        raise
    except Exception as e:
        print(f"发送PX4任务失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="发送任务时发生错误"
        )

# 添加用于web页面直接访问的接口
@router.get("/pose/{drone_id}")
def get_uav1_telemetry_pose_web(
    drone_id: str = Path(..., description="无人机ID"),
    telemetry_type: str = Query("all", description="遥测类型: all, pose, twist, battery, gps, imu, alert, platform_info, batch")
) -> Dict[str, Any]:
    print(f"[DEBUG] 接收到Web获取遥测数据请求，无人机ID: {drone_id}, 类型: {telemetry_type}")
    """
    为web页面提供的无人机遥测数据接口

    直接返回无人机的各种遥测数据，无需认证，方便前端页面访问
    支持获取单一类型或所有遥测数据

    Args:
        drone_id: 无人机ID
        telemetry_type: 遥测数据类型，默认为 all（所有数据）

    Returns:
        无人机遥测数据
    """
    try:
        # 初始化Zenoh客户端
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 定义遥测数据类型映射
        telemetry_types = {
            "pose": "pose",
            "twist": "twist",
            "battery": "battery",
            "gps": "gps",
            "imu": "imu",
            "alert": "alert",
            "platform_info": "platform_info"
        }

        # 如果请求批量数据
        if telemetry_type == "batch":
            batch_key = f"web/{drone_id}/telemetry/batch"
            response = zenoh_client.get(batch_key)

            if response["success"]:
                payload_text = zenoh_client.extract_payload(response["text"])
                return json.loads(payload_text) if payload_text else {}
            else:
                return {"error": "无法获取批量遥测数据", "drone_id": drone_id}

        # 如果请求特定类型
        elif telemetry_type in telemetry_types:
            telemetry_key = f"web/{drone_id}/telemetry/{telemetry_types[telemetry_type]}"
            response = zenoh_client.get(telemetry_key)

            if response["success"]:
                payload_text = zenoh_client.extract_payload(response["text"])
                raw_data = json.loads(payload_text) if payload_text else {}
                return extract_telemetry_data(raw_data, telemetry_type)
            else:
                return get_default_telemetry_data(telemetry_type)

        # 如果请求所有数据
        elif telemetry_type == "all":
            all_data = {}

            # 获取所有类型的遥测数据
            for telem_type, telem_name in telemetry_types.items():
                telemetry_key = f"web/{drone_id}/telemetry/{telem_name}"
                response = zenoh_client.get(telemetry_key)

                if response["success"]:
                    try:
                        payload_text = zenoh_client.extract_payload(response["text"])
                        raw_data = json.loads(payload_text) if payload_text else {}
                        all_data[telem_type] = extract_telemetry_data(raw_data, telem_type)
                    except:
                        all_data[telem_type] = get_default_telemetry_data(telem_type)
                else:
                    all_data[telem_type] = get_default_telemetry_data(telem_type)

            # 添加时间戳
            all_data["timestamp"] = datetime.now().isoformat()
            all_data["drone_id"] = drone_id

            return all_data

        else:
            return {"error": f"不支持的遥测类型: {telemetry_type}"}

    except Exception as e:
        print(f"[ERROR] 获取遥测数据失败: {str(e)}")
        # 返回默认数据作为备用
        if telemetry_type == "all":
            return {
                "pose": get_default_telemetry_data("pose"),
                "twist": get_default_telemetry_data("twist"),
                "battery": get_default_telemetry_data("battery"),
                "gps": get_default_telemetry_data("gps"),
                "imu": get_default_telemetry_data("imu"),
                "alert": None,
                "platform_info": None,
                "timestamp": datetime.now().isoformat(),
                "drone_id": drone_id
            }
        else:
            return get_default_telemetry_data(telemetry_type)

@router.get("/telemetry/{drone_id}/all", response_model=ResponseModel[Dict[str, Any]])
def get_all_telemetry_data(
    drone_id: str = Path(..., description="无人机ID"),
    use_batch: bool = Query(False, description="是否使用批量数据接口"),
    current_user: User = Depends(get_current_active_user)
) -> Any:
    """
    获取无人机的所有遥测数据

    通过Zenoh REST接口获取无人机的所有遥测数据，包括：
    - pose: 位置姿态数据
    - twist: 速度数据
    - battery: 电池状态
    - gps: GPS定位信息
    - imu: IMU传感器数据
    - alert: 警报事件
    - platform_info: 平台信息

    Args:
        drone_id: 无人机ID
        use_batch: 是否使用批量数据接口（如果为True，则从batch端点获取）
        current_user: 当前用户

    Returns:
        包含所有遥测数据的字典
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        if use_batch:
            # 使用批量数据接口
            batch_key = f"web/{drone_id}/telemetry/batch"
            response = zenoh_client.get(batch_key)

            if response["success"]:
                payload_text = zenoh_client.extract_payload(response["text"])
                batch_data = json.loads(payload_text) if payload_text else {}

                # 如果批量数据包含topics字段，处理并简化数据
                if "topics" in batch_data:
                    simplified_topics = {}
                    for topic_name, topic_data in batch_data["topics"].items():
                        simplified_topics[topic_name] = extract_telemetry_data(topic_data, topic_name)

                    return ResponseModel(data={
                        "drone_id": drone_id,
                        "timestamp": batch_data.get("ts", datetime.now().timestamp()),
                        "data": simplified_topics
                    })
                return ResponseModel(data=batch_data)
            else:
                return ResponseModel(data={
                    "error": "无法获取批量遥测数据",
                    "drone_id": drone_id
                })

        # 定义所有遥测类型
        telemetry_types = [
            ("pose", "pose"),
            ("twist", "twist"),
            ("battery", "battery"),
            ("gps", "gps"),
            ("imu", "imu"),
            ("alert", "alert"),
            ("platform_info", "platform_info")
        ]

        all_telemetry = {}

        # 逐个获取每种遥测数据
        for data_type, key_name in telemetry_types:
            telemetry_key = f"web/{drone_id}/telemetry/{key_name}"
            response = zenoh_client.get(telemetry_key)

            if response["success"]:
                try:
                    payload_text = zenoh_client.extract_payload(response["text"])
                    raw_data = json.loads(payload_text) if payload_text else {}
                    all_telemetry[data_type] = extract_telemetry_data(raw_data, data_type)
                except Exception as e:
                    print(f"[DEBUG] 解析{data_type}数据失败: {str(e)}")
                    all_telemetry[data_type] = get_default_telemetry_data(data_type)
            else:
                all_telemetry[data_type] = get_default_telemetry_data(data_type)

        return ResponseModel(data={
            "drone_id": drone_id,
            "timestamp": datetime.now().isoformat(),
            "data": all_telemetry
        })

    except Exception as e:
        print(f"获取所有遥测数据失败: {str(e)}")
        # 返回默认数据
        return ResponseModel(data={
            "drone_id": drone_id,
            "timestamp": datetime.now().isoformat(),
            "data": {
                "pose": get_default_telemetry_data("pose"),
                "twist": get_default_telemetry_data("twist"),
                "battery": get_default_telemetry_data("battery"),
                "gps": get_default_telemetry_data("gps"),
                "imu": get_default_telemetry_data("imu"),
                "alert": get_default_telemetry_data("alert"),
                "platform_info": get_default_telemetry_data("platform_info")
            }
        })

@router.get("/monitoring/status/{drone_id}")
async def get_drone_monitoring_status(
    drone_id: str = Path(..., description="无人机ID"),
    current_user: User = Depends(get_current_active_user)
) -> ResponseModel[Dict[str, Any]]:
    """
    获取无人机监控状态信息

    Args:
        drone_id: 无人机ID
        current_user: 当前活跃用户

    Returns:
        无人机监控状态信息
    """
    if drone_id not in monitored_drones:
        return ResponseModel(
            data={
                "is_monitoring": False,
                "message": "无人机未在监控中"
            }
        )

    return ResponseModel(
        data=monitored_drones[drone_id]
    )

# ==================== 无人机控制命令接口 ====================
# 基于 downstream_commands.py 的命令配置实现

@router.post("/control/takeoff", response_model=ResponseModel[Dict[str, Any]])
async def control_takeoff(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    height: float = Body(10.0, description="起飞高度(米)"),
    speed: float = Body(1.0, description="起飞速度(米/秒)"),
    cmd_id: Optional[str] = Body(None, description="命令ID，用于去重和幂等"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    """
    控制无人机起飞

    发送起飞命令到指定的无人机

    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        height: 起飞高度(米)
        speed: 起飞速度(米/秒)
        cmd_id: 命令ID，用于去重和幂等
        current_user: 当前用户
        db: 数据库会话

    Returns:
        命令执行结果
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 构建命令载荷
        payload = {
            "height": height,
            "speed": speed,
            "wait": True
        }

        # 如果提供了cmd_id，添加到载荷
        if cmd_id:
            payload["cmd_id"] = cmd_id

        # 构建Zenoh键
        zenoh_key = f"rt/{drone_id}/control/takeoff"

        # 发送命令
        response = zenoh_client.put(zenoh_key, payload)

        if response["success"]:
            # 记录操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="control_takeoff",
                operation_content=f"用户 {current_user.username} 控制无人机 {drone_id} 起飞到 {height}米",
                success=1
            )

            return ResponseModel(data={
                "success": True,
                "message": "起飞命令已发送",
                "drone_id": drone_id,
                "cmd_id": cmd_id
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": f"发送命令失败: {response.get('error', '未知错误')}",
                "drone_id": drone_id
            })

    except Exception as e:
        print(f"起飞命令失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="发送起飞命令时发生错误"
        )

@router.post("/control/land", response_model=ResponseModel[Dict[str, Any]])
async def control_land(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    speed: float = Body(1.0, description="降落速度(米/秒)"),
    cmd_id: Optional[str] = Body(None, description="命令ID，用于去重和幂等"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    """
    控制无人机降落

    发送降落命令到指定的无人机

    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        speed: 降落速度(米/秒)
        cmd_id: 命令ID，用于去重和幂等
        current_user: 当前用户
        db: 数据库会话

    Returns:
        命令执行结果
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 构建命令载荷
        payload = {
            "speed": speed,
            "wait": True
        }

        # 如果提供了cmd_id，添加到载荷
        if cmd_id:
            payload["cmd_id"] = cmd_id

        # 构建Zenoh键
        zenoh_key = f"rt/{drone_id}/control/land"

        # 发送命令
        response = zenoh_client.put(zenoh_key, payload)

        if response["success"]:
            # 记录操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="control_land",
                operation_content=f"用户 {current_user.username} 控制无人机 {drone_id} 降落",
                success=1
            )

            return ResponseModel(data={
                "success": True,
                "message": "降落命令已发送",
                "drone_id": drone_id,
                "cmd_id": cmd_id
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": f"发送命令失败: {response.get('error', '未知错误')}",
                "drone_id": drone_id
            })

    except Exception as e:
        print(f"降落命令失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="发送降落命令时发生错误"
        )

@router.post("/control/goto", response_model=ResponseModel[Dict[str, Any]])
async def control_goto_waypoint(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    x: float = Body(..., description="目标X坐标(米)"),
    y: float = Body(..., description="目标Y坐标(米)"),
    z: float = Body(..., description="目标Z坐标/高度(米)"),
    speed: float = Body(3.0, description="飞行速度(米/秒)"),
    yaw_mode: int = Body(0, description="偏航模式"),
    yaw_angle: float = Body(0.0, description="偏航角度(弧度)"),
    cmd_id: Optional[str] = Body(None, description="命令ID，用于去重和幂等"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    """
    控制无人机前往指定航点

    发送GoTo命令到指定的无人机，使其飞往目标位置

    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        x: 目标X坐标(米)
        y: 目标Y坐标(米)
        z: 目标Z坐标/高度(米)
        speed: 飞行速度(米/秒)
        yaw_mode: 偏航模式
        yaw_angle: 偏航角度(弧度)
        cmd_id: 命令ID，用于去重和幂等
        current_user: 当前用户
        db: 数据库会话

    Returns:
        命令执行结果
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 构建命令载荷
        payload = {
            "pose": {
                "position": {"x": x, "y": y, "z": z},
                "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
            },
            "speed": speed,
            "yaw_mode": yaw_mode,
            "yaw_angle": yaw_angle,
            "wait": True
        }

        # 如果提供了cmd_id，添加到载荷
        if cmd_id:
            payload["cmd_id"] = cmd_id

        # 构建Zenoh键
        zenoh_key = f"rt/{drone_id}/control/goto"

        # 发送命令
        response = zenoh_client.put(zenoh_key, payload)

        if response["success"]:
            # 记录操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="control_goto",
                operation_content=f"用户 {current_user.username} 控制无人机 {drone_id} 前往位置 ({x:.1f}, {y:.1f}, {z:.1f})",
                success=1
            )

            return ResponseModel(data={
                "success": True,
                "message": "GoTo命令已发送",
                "drone_id": drone_id,
                "target": {"x": x, "y": y, "z": z},
                "cmd_id": cmd_id
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": f"发送命令失败: {response.get('error', '未知错误')}",
                "drone_id": drone_id
            })

    except Exception as e:
        print(f"GoTo命令失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="发送GoTo命令时发生错误"
        )

@router.post("/control/follow_path", response_model=ResponseModel[Dict[str, Any]])
async def control_follow_path(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    path: List[Dict[str, float]] = Body(..., description="路径点列表"),
    speed: float = Body(3.0, description="飞行速度(米/秒)"),
    yaw_mode: int = Body(0, description="偏航模式"),
    yaw_angle: float = Body(0.0, description="偏航角度(弧度)"),
    mission_id: Optional[str] = Body(None, description="任务ID，用于去重和幂等"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    """
    控制无人机跟随路径飞行

    发送路径跟随命令到指定的无人机

    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        path: 路径点列表，每个点包含x,y,z坐标
        speed: 飞行速度(米/秒)
        yaw_mode: 偏航模式
        yaw_angle: 偏航角度(弧度)
        mission_id: 任务ID，用于去重和幂等
        current_user: 当前用户
        db: 数据库会话

    Returns:
        命令执行结果
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 转换路径格式
        waypoints = []
        for point in path:
            waypoints.append({
                "pose": {
                    "position": {
                        "x": point.get("x", 0.0),
                        "y": point.get("y", 0.0),
                        "z": point.get("z", 0.0)
                    },
                    "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
                }
            })

        # 构建命令载荷
        payload = {
            "path": waypoints,
            "speed": speed,
            "yaw_mode": yaw_mode,
            "yaw_angle": yaw_angle,
            "wait": True
        }

        # 如果提供了mission_id，添加到载荷
        if mission_id:
            payload["mission_id"] = mission_id

        # 构建Zenoh键
        zenoh_key = f"rt/{drone_id}/control/follow_path"

        # 发送命令
        response = zenoh_client.put(zenoh_key, payload)

        if response["success"]:
            # 记录操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="control_follow_path",
                operation_content=f"用户 {current_user.username} 控制无人机 {drone_id} 跟随包含 {len(path)} 个航点的路径",
                success=1
            )

            return ResponseModel(data={
                "success": True,
                "message": "路径跟随命令已发送",
                "drone_id": drone_id,
                "waypoints_count": len(path),
                "mission_id": mission_id
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": f"发送命令失败: {response.get('error', '未知错误')}",
                "drone_id": drone_id
            })

    except Exception as e:
        print(f"路径跟随命令失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="发送路径跟随命令时发生错误"
        )

# ==================== 无人机服务命令接口 ====================

@router.post("/service/set_arming_state", response_model=ResponseModel[Dict[str, Any]])
async def service_set_arming_state(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    armed: bool = Body(..., description="解锁状态: True=解锁, False=锁定"),
    cmd_id: Optional[str] = Body(None, description="命令ID，用于去重和幂等"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    """
    设置无人机解锁/锁定状态

    发送解锁或锁定命令到指定的无人机

    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        armed: 解锁状态，True表示解锁，False表示锁定
        cmd_id: 命令ID，用于去重和幂等
        current_user: 当前用户
        db: 数据库会话

    Returns:
        命令执行结果
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 构建命令载荷（符合SetBool服务格式）
        payload = {
            "data": armed
        }

        # 如果提供了cmd_id，添加到载荷
        if cmd_id:
            payload["cmd_id"] = cmd_id

        # 构建Zenoh键
        zenoh_key = f"rt/{drone_id}/service/set_arming_state"

        # 发送命令
        response = zenoh_client.put(zenoh_key, payload)

        if response["success"]:
            # 记录操作
            action = "解锁" if armed else "锁定"
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="service_arming",
                operation_content=f"用户 {current_user.username} {action}无人机 {drone_id}",
                success=1
            )

            return ResponseModel(data={
                "success": True,
                "message": f"{action}命令已发送",
                "drone_id": drone_id,
                "armed": armed,
                "cmd_id": cmd_id
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": f"发送命令失败: {response.get('error', '未知错误')}",
                "drone_id": drone_id
            })

    except Exception as e:
        print(f"设置解锁状态失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="设置解锁状态时发生错误"
        )

@router.post("/service/platform_takeoff", response_model=ResponseModel[Dict[str, Any]])
async def service_platform_takeoff(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    enable: bool = Body(True, description="启用平台起飞"),
    cmd_id: Optional[str] = Body(None, description="命令ID，用于去重和幂等"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    """
    平台起飞服务

    通过平台服务控制无人机起飞

    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        enable: 是否启用平台起飞
        cmd_id: 命令ID，用于去重和幂等
        current_user: 当前用户
        db: 数据库会话

    Returns:
        命令执行结果
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 构建命令载荷（符合SetBool服务格式）
        payload = {
            "data": enable
        }

        # 如果提供了cmd_id，添加到载荷
        if cmd_id:
            payload["cmd_id"] = cmd_id

        # 构建Zenoh键
        zenoh_key = f"rt/{drone_id}/service/platform_takeoff"

        # 发送命令
        response = zenoh_client.put(zenoh_key, payload)

        if response["success"]:
            # 记录操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="service_platform_takeoff",
                operation_content=f"用户 {current_user.username} 触发无人机 {drone_id} 平台起飞",
                success=1
            )

            return ResponseModel(data={
                "success": True,
                "message": "平台起飞命令已发送",
                "drone_id": drone_id,
                "cmd_id": cmd_id
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": f"发送命令失败: {response.get('error', '未知错误')}",
                "drone_id": drone_id
            })

    except Exception as e:
        print(f"平台起飞命令失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="平台起飞时发生错误"
        )

@router.post("/service/platform_land", response_model=ResponseModel[Dict[str, Any]])
async def service_platform_land(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    enable: bool = Body(True, description="启用平台降落"),
    cmd_id: Optional[str] = Body(None, description="命令ID，用于去重和幂等"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    """
    平台降落服务

    通过平台服务控制无人机降落

    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        enable: 是否启用平台降落
        cmd_id: 命令ID，用于去重和幂等
        current_user: 当前用户
        db: 数据库会话

    Returns:
        命令执行结果
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 构建命令载荷（符合SetBool服务格式）
        payload = {
            "data": enable
        }

        # 如果提供了cmd_id，添加到载荷
        if cmd_id:
            payload["cmd_id"] = cmd_id

        # 构建Zenoh键
        zenoh_key = f"rt/{drone_id}/service/platform_land"

        # 发送命令
        response = zenoh_client.put(zenoh_key, payload)

        if response["success"]:
            # 记录操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="service_platform_land",
                operation_content=f"用户 {current_user.username} 触发无人机 {drone_id} 平台降落",
                success=1
            )

            return ResponseModel(data={
                "success": True,
                "message": "平台降落命令已发送",
                "drone_id": drone_id,
                "cmd_id": cmd_id
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": f"发送命令失败: {response.get('error', '未知错误')}",
                "drone_id": drone_id
            })

    except Exception as e:
        print(f"平台降落命令失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="平台降落时发生错误"
        )

@router.post("/service/set_control_mode", response_model=ResponseModel[Dict[str, Any]])
async def service_set_control_mode(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    control_mode: int = Body(..., description="控制模式"),
    reference_frame: Optional[int] = Body(None, description="参考坐标系"),
    yaw_mode: Optional[int] = Body(None, description="偏航模式"),
    cmd_id: Optional[str] = Body(None, description="命令ID，用于去重和幂等"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    """
    设置无人机控制模式（支持简化格式）

    支持两种格式：
    1. 简化格式: {"control_mode": 2, "reference_frame": 1, "yaw_mode": 0}
    2. 嵌套格式: {"control_mode": {"control_mode": 2, "reference_frame": 1, "yaw_mode": 0}}

    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        control_mode: 控制模式值
        reference_frame: 参考坐标系（可选）
        yaw_mode: 偏航模式（可选）
        cmd_id: 命令ID，用于去重和幂等
        current_user: 当前用户
        db: 数据库会话

    Returns:
        命令执行结果
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 构建命令载荷（简化格式）
        payload = {
            "control_mode": control_mode
        }

        # 添加可选字段
        if reference_frame is not None:
            payload["reference_frame"] = reference_frame
        if yaw_mode is not None:
            payload["yaw_mode"] = yaw_mode
        if cmd_id:
            payload["cmd_id"] = cmd_id

        # 构建Zenoh键
        zenoh_key = f"rt/{drone_id}/service/set_control_mode"

        # 发送命令
        response = zenoh_client.put(zenoh_key, payload)

        if response["success"]:
            # 记录操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="service_control_mode",
                operation_content=f"用户 {current_user.username} 设置无人机 {drone_id} 控制模式为 {control_mode}",
                success=1
            )

            return ResponseModel(data={
                "success": True,
                "message": "控制模式设置命令已发送",
                "drone_id": drone_id,
                "control_mode": control_mode,
                "cmd_id": cmd_id
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": f"发送命令失败: {response.get('error', '未知错误')}",
                "drone_id": drone_id
            })

    except Exception as e:
        print(f"设置控制模式失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="设置控制模式时发生错误"
        )

# ==================== 命令结果查询接口 ====================

@router.get("/command/result/{drone_id}/{command_type}", response_model=ResponseModel[Dict[str, Any]])
async def get_command_result(
    drone_id: str = Path(..., description="无人机ID"),
    command_type: str = Path(..., description="命令类型: takeoff, land, goto, follow_path, arming, control_mode等"),
    cmd_id: Optional[str] = Query(None, description="命令ID，用于查询特定命令的结果"),
    current_user: User = Depends(get_current_active_user)
) -> Any:
    """
    查询命令执行结果

    获取之前发送的命令的执行状态和结果

    Args:
        drone_id: 无人机ID
        command_type: 命令类型
        cmd_id: 命令ID（可选）
        current_user: 当前用户

    Returns:
        命令执行结果
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 构建结果查询键
        if command_type in ["takeoff", "land", "goto", "follow_path"]:
            result_key = f"web/{drone_id}/control/{command_type}/result"
        elif command_type in ["set_arming_state", "platform_takeoff", "platform_land", "set_control_mode"]:
            result_key = f"web/{drone_id}/service/{command_type}/result"
        else:
            result_key = f"web/{drone_id}/{command_type}/result"

        # 查询结果
        response = zenoh_client.get(result_key)

        if response["success"]:
            payload_text = zenoh_client.extract_payload(response["text"])
            result = json.loads(payload_text) if payload_text else {}

            # 如果指定了cmd_id，检查是否匹配
            if cmd_id and result.get("cmd_id") != cmd_id:
                return ResponseModel(data={
                    "success": False,
                    "message": "未找到指定命令的结果",
                    "drone_id": drone_id,
                    "command_type": command_type,
                    "cmd_id": cmd_id
                })

            return ResponseModel(data={
                "success": True,
                "drone_id": drone_id,
                "command_type": command_type,
                "result": result
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": "无法获取命令结果",
                "drone_id": drone_id,
                "command_type": command_type
            })

    except Exception as e:
        print(f"查询命令结果失败: {str(e)}")
        return ResponseModel(data={
            "success": False,
            "message": "查询命令结果时发生错误",
            "error": str(e)
        })

# ==================== 实时位姿控制接口 (Topic) ====================

@router.post("/control/publish_pose", response_model=ResponseModel[Dict[str, Any]])
async def control_publish_pose(
    request: Request,
    drone_id: str = Body(..., description="无人机ID"),
    pose: Dict[str, Dict[str, float]] = Body(..., description="位姿数据"),
    frame_id: str = Body("map", description="坐标系ID"),
    cmd_id: Optional[str] = Body(None, description="命令ID，用于去重"),
    current_user: User = Depends(get_current_active_user),
    db: Session = Depends(get_db)
) -> Any:
    """
    发布位姿到无人机（Topic 模式）

    适用于实时轨迹跟踪、手动遥控等高频控制场景

    Args:
        request: HTTP请求对象
        drone_id: 无人机ID
        pose: 位姿数据 {"position": {"x": 0, "y": 0, "z": 0}, "orientation": {...}}
        frame_id: 坐标系ID，默认 "map"
        cmd_id: 命令ID，用于去重
        current_user: 当前用户
        db: 数据库会话

    Returns:
        命令执行结果（立即返回）
    """
    try:
        zenoh_client = ZenohClient(ZENOH_BASE_URL)

        # 构建 PoseStamped 格式的载荷
        payload = {
            "header": {
                "stamp": {
                    "sec": int(datetime.now().timestamp()),
                    "nanosec": 0
                },
                "frame_id": frame_id
            },
            "pose": pose
        }

        if cmd_id:
            payload["cmd_id"] = cmd_id

        # 构建 Zenoh 键（Topic）
        zenoh_key = f"web/{drone_id}/control/pose"

        # 发送命令
        response = zenoh_client.put(zenoh_key, payload)

        if response["success"]:
            # 记录操作
            log_operation(
                db=db,
                user_id=current_user.id,
                username=current_user.username,
                ip_address=get_client_ip(request),
                operation_type="control_publish_pose",
                operation_content=f"用户 {current_user.username} 发布位姿到无人机 {drone_id}",
                success=1
            )

            return ResponseModel(data={
                "success": True,
                "message": "位姿已发布",
                "drone_id": drone_id,
                "cmd_id": cmd_id
            })
        else:
            return ResponseModel(data={
                "success": False,
                "message": f"发送失败: {response.get('error', '未知错误')}",
                "drone_id": drone_id
            })

    except Exception as e:
        print(f"发布位姿失败: {str(e)}")
        raise HTTPException(
            status_code=500,
            detail="发布位姿时发生错误"
        )
